#include <jni.h>
#include <android/log.h>



#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>
#include <camera_properties.h>
#include <cvctracker/cvc_tracker.h>


#include <GLES2/gl2.h>
#include <GLES2/gl2ext.h>

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "com_cvc_tracker_es_CVCTLib.h"

#define  LOG_TAG    "CVCTLibJni"
#define  LOGI(...)  __android_log_print(ANDROID_LOG_INFO,LOG_TAG,__VA_ARGS__)
#define  LOGE(...)  __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__)


///////////////////////////////
cv::VideoCapture *_cap;
cv::Mat _frameGrabbed;
GLuint _textureGLID;
GLuint gProgram;
GLuint gvPositionHandle;
GLuint gvTextureHandle;
GLuint gvSamplerHandle;
GLuint gCubeProgram;
GLint iLocPosition = 0;
GLint iLocColour, iLocMVP;

#if 1
cv::SurfFeatureDetector detector(600);
cv::SurfDescriptorExtractor extractor;
#else
orb
#endif
cv::DescriptorMatcher *matcher  = new cv::BFMatcher(cv::NORM_L2,true);
CVCTracker cvct(&detector , &extractor , matcher);
cv::Mat imgPool[2];
int currImage = 0;
float mvp[16];
float mvpRender[16];
		
//////////////////////////////



static void checkGlError(const char* op) 
{
    for (GLint error = glGetError(); error; error = glGetError()) 
    {
        LOGI("after %s() glError (0x%x)\n", op, error);
    }
}

static void printGLString(const char *name, GLenum s) 
{
    const char *v = (const char *) glGetString(s);
    LOGI("GL %s = %s\n", name, v);
}


static const char strVShader[] =
    "attribute vec4 vPosition;												\n" 
    "attribute vec2 vUV;													\n" 
    "varying vec2 v_texCoords;												\n" 
    "void main()															\n" 
    "{																		\n" 
          "gl_Position = vPosition;											\n" 
          "v_texCoords = vUV;												\n" 
    "}																		\n";



static const char strFShader[] =
    "precision mediump float;												\n" 
    "varying vec2 v_texCoords;												\n" 
    "uniform sampler2D myTextureSampler;									\n" 
    "void main()															\n" 
    "{																		\n" 
      "gl_FragColor = texture2D(myTextureSampler, v_texCoords);				\n" 
    "}																		\n";



static const char fragmentShader[] =    
	"precision lowp float; 														\n"
	"varying vec3 vv3colour; 													\n"
	"																			\n"
	"void main() 																\n"
	"{																			\n"
	"	gl_FragColor = vec4(vv3colour, 1.0);									\n"
	"}																			\n";


static const char vertexShader[] = 
	"attribute vec4 av4position;												\n"
	"attribute vec3 av3colour;												    \n"
	"																			\n"
	"uniform mat4 mvp;															\n"
	"varying vec3 vv3colour;													\n"
	"																			\n"
	"void main() 																\n"
	"{																			\n"
	"	vv3colour = av3colour;													\n"
	"	gl_Position = mvp * av4position;										\n"
	"}																			\n";				



static const GLfloat  squareVertices[]  = {
    -1.0f, -1.0f,
     1.0f, -1.0f,
    -1.0f,  1.0f,
     1.0f,  1.0f,
};

static const GLfloat  textureVertices[] = {
     0.0f, 1.0f,
     1.0f, 1.0f,
     0.0f, 0.0f,
     1.0f, 0.0f,
};



const float g_vertex_buffer_datass[] =
{
    /* Front face. */
    /* Bottom left */
    -0.5,  0.5, -0.5,
    0.5, -0.5, -0.5,
    -0.5, -0.5, -0.5,
    /* Top right */
    -0.5,  0.5, -0.5,
    0.5,  0.5, -0.5,
    0.5, -0.5, -0.5,
    /* Left face */
    /* Bottom left */
    -0.5,  0.5,  0.5,
    -0.5, -0.5, -0.5,
    -0.5, -0.5,  0.5,
    /* Top right */
    -0.5,  0.5,  0.5,
    -0.5,  0.5, -0.5,
    -0.5, -0.5, -0.5,
    /* Top face */
    /* Bottom left */
    -0.5,  0.5,  0.5,
    0.5,  0.5, -0.5,
    -0.5,  0.5, -0.5,
    /* Top right */
    -0.5,  0.5,  0.5,
    0.5,  0.5,  0.5,
    0.5,  0.5, -0.5,
    /* Right face */
    /* Bottom left */
    0.5,  0.5, -0.5,
    0.5, -0.5,  0.5,
    0.5, -0.5, -0.5,
    /* Top right */
    0.5,  0.5, -0.5,
    0.5,  0.5,  0.5,
    0.5, -0.5,  0.5,
    /* Back face */
    /* Bottom left */
    0.5,  0.5,  0.5,
    -0.5, -0.5,  0.5,
    0.5, -0.5,  0.5,
    /* Top right */
    0.5,  0.5,  0.5,
    -0.5,  0.5,  0.5,
    -0.5, -0.5,  0.5,
    /* Bottom face */
    /* Bottom left */
    -0.5, -0.5, -0.5,
    0.5, -0.5,  0.5,
    -0.5, -0.5,  0.5,
    /* Top right */
    -0.5, -0.5, -0.5,
    0.5, -0.5, -0.5,
    0.5, -0.5,  0.5,
};


const float g_vertex_buffer_data[] =
{
    /* Front face. */
    /* Bottom left */
    -8.0,  8.0, -8.0,
    8.0, -8.0, -8.0,
    -8.0, -8.0, -8.0,
    /* Top right */
    -8.0,  8.0, -8.0,
    8.0,  8.0, -8.0,
    8.0, -8.0, -8.0,
    /* Left face */
    /* Bottom left */
    -8.0,  8.0,  8.0,
    -8.0, -8.0, -8.0,
    -8.0, -8.0,  8.0,
    /* Top right */
    -8.0,  8.0,  8.0,
    -8.0,  8.0, -8.0,
    -8.0, -8.0, -8.0,
    /* Top face */
    /* Bottom left */
    -8.0,  8.0,  8.0,
    8.0,  8.0, -8.0,
    -8.0,  8.0, -8.0,
    /* Top right */
    -8.0,  8.0,  8.0,
    8.0,  8.0,  8.0,
    8.0,  8.0, -8.0,
    /* Right face */
    /* Bottom left */
    8.0,  8.0, -8.0,
    8.0, -8.0,  8.0,
    8.0, -8.0, -8.0,
    /* Top right */
    8.0,  8.0, -8.0,
    8.0,  8.0,  8.0,
    8.0, -8.0,  8.0,
    /* Back face */
    /* Bottom left */
    8.0,  8.0,  8.0,
    -8.0, -8.0,  8.0,
    8.0, -8.0,  8.0,
    /* Top right */
    8.0,  8.0,  8.0,
    -8.0,  8.0,  8.0,
    -8.0, -8.0,  8.0,
    /* Bottom face */
    /* Bottom left */
    -8.0, -8.0, -8.0,
    8.0, -8.0,  8.0,
    -8.0, -8.0,  8.0,
    /* Top right */
    -8.0, -8.0, -8.0,
    8.0, -8.0, -8.0,
    8.0, -8.0,  8.0,
};


const float g_color_buffer_data[] =
{
    /* Front face */
    /* Bottom left */
    1.0, 0.0, 0.0, /* red */
    0.0, 0.0, 1.0, /* blue */
    0.0, 1.0, 0.0, /* green */
    /* Top right */
    1.0, 0.0, 0.0, /* red */
    1.0, 1.0, 0.0, /* yellow */
    0.0, 0.0, 1.0, /* blue */
    /* Left face */
    /* Bottom left */
    1.0, 1.0, 1.0, /* white */
    0.0, 1.0, 0.0, /* green */
    0.0, 1.0, 1.0, /* cyan */
    /* Top right */
    1.0, 1.0, 1.0, /* white */
    1.0, 0.0, 0.0, /* red */
    0.0, 1.0, 0.0, /* green */
    /* Top face */
    /* Bottom left */
    1.0, 1.0, 1.0, /* white */
    1.0, 1.0, 0.0, /* yellow */
    1.0, 0.0, 0.0, /* red */
    /* Top right */
    1.0, 1.0, 1.0, /* white */
    0.0, 0.0, 0.0, /* black */
    1.0, 1.0, 0.0, /* yellow */
    /* Right face */
    /* Bottom left */
    1.0, 1.0, 0.0, /* yellow */
    1.0, 0.0, 1.0, /* magenta */
    0.0, 0.0, 1.0, /* blue */
    /* Top right */
    1.0, 1.0, 0.0, /* yellow */
    0.0, 0.0, 0.0, /* black */
    1.0, 0.0, 1.0, /* magenta */
    /* Back face */
    /* Bottom left */
    0.0, 0.0, 0.0, /* black */
    0.0, 1.0, 1.0, /* cyan */
    1.0, 0.0, 1.0, /* magenta */
    /* Top right */
    0.0, 0.0, 0.0, /* black */
    1.0, 1.0, 1.0, /* white */
    0.0, 1.0, 1.0, /* cyan */
    /* Bottom face */
    /* Bottom left */
    0.0, 1.0, 0.0, /* green */
    1.0, 0.0, 1.0, /* magenta */
    0.0, 1.0, 1.0, /* cyan */
    /* Top right */
    0.0, 1.0, 0.0, /* green */
    0.0, 0.0, 1.0, /* blue */
    1.0, 0.0, 1.0, /* magenta */
};


GLuint loadShader(GLenum shaderType, const char* pSource) 
{
    GLuint shader = glCreateShader(shaderType);
    if (shader) 
    {
        glShaderSource(shader, 1, &pSource, NULL);
        glCompileShader(shader);
        GLint compiled = 0;
        glGetShaderiv(shader, GL_COMPILE_STATUS, &compiled);
        if (!compiled) 
        {
            GLint infoLen = 0;
            glGetShaderiv(shader, GL_INFO_LOG_LENGTH, &infoLen);
            if (infoLen) 
            {
                char* buf = (char*) malloc(infoLen);
                if (buf) 
                {
                    glGetShaderInfoLog(shader, infoLen, NULL, buf);
                    LOGE("Could not compile shader %d:\n%s\n", shaderType, buf);
                    free(buf);
                }
                glDeleteShader(shader);
                shader = 0;
            }
        }
    }
    return shader;
}



GLuint createProgram(const char* pVertexSource, const char* pFragmentSource) 
{
    GLuint vertexShader = loadShader(GL_VERTEX_SHADER, pVertexSource);
    if (!vertexShader)
        return 0;

    GLuint pixelShader = loadShader(GL_FRAGMENT_SHADER, pFragmentSource);
    if (!pixelShader)
        return 0;

    GLuint program = glCreateProgram();
    if (program) 
    {
        glAttachShader(program, vertexShader);
        checkGlError("glAttachShader");
        glAttachShader(program, pixelShader);
        checkGlError("glAttachShader");
        glLinkProgram(program);
        GLint linkStatus = GL_FALSE;
        glGetProgramiv(program, GL_LINK_STATUS, &linkStatus);
        if (linkStatus != GL_TRUE) 
        {
            GLint bufLength = 0;
            glGetProgramiv(program, GL_INFO_LOG_LENGTH, &bufLength);
            if (bufLength) 
            {
                char* buf = (char*) malloc(bufLength);
                if (buf) 
                {
                    glGetProgramInfoLog(program, bufLength, NULL, buf);
                    LOGE("Could not link program:\n%s\n", buf);
                    free(buf);
                }
            }
            glDeleteProgram(program);
            program = 0;
        }
    }
    return program;
}


bool setupGraphics(int w, int h) 
{
    printGLString("Version", GL_VERSION);
    printGLString("Vendor", GL_VENDOR);
    printGLString("Renderer", GL_RENDERER);
    printGLString("Extensions", GL_EXTENSIONS);

    LOGI("setupGraphics(%d, %d)", w, h);
    gProgram = createProgram(strVShader,strFShader);
    if (!gProgram) 
    {
        LOGE("Could not create program.");
        return false;
    }
    gvPositionHandle = glGetAttribLocation(gProgram, "vPosition");
	checkGlError("glGetAttribLocation");
    LOGI("glGetAttribLocation(\"vPosition\") = %d\n", gvPositionHandle);
	gvTextureHandle = glGetAttribLocation(gProgram, "vUV");
    checkGlError("glGetAttribLocation");
    LOGI("glGetAttribLocation(\"vUV\") = %d\n", gvTextureHandle);

	gvSamplerHandle = glGetUniformLocation(gProgram, "myTextureSampler");
	
	gCubeProgram = createProgram( vertexShader, fragmentShader ); 
    iLocPosition = glGetAttribLocation(gCubeProgram, "av4position");
    iLocColour = glGetAttribLocation(gCubeProgram, "av3colour");
    iLocMVP = glGetUniformLocation(gCubeProgram, "mvp");

    glViewport(0, 0, w, h);
    checkGlError("glViewport");
    
    return true;
}




////////////////////////////////////////////////////////////


/*
 * Class:     com_cvc_tracker_es_CVCTLib
 * Method:    initCam
 * Signature: ()V
 */
JNIEXPORT int JNICALL Java_com_cvc_tracker_es_CVCTLib_initCam
  (JNIEnv *, jclass)
  {
	LOGI("Java_com_cvc_tracker_es_CVCTLib_initCam");
	_cap = new cv::VideoCapture();
	_cap->set(CV_CAP_PROP_CONVERT_RGB, 1);
	
	_cap->open(0);
	_cap->set(CV_CAP_PROP_FRAME_WIDTH , 640);
	_cap->set(CV_CAP_PROP_FRAME_HEIGHT , 480);
	_cap->set(CV_CAP_PROP_ANDROID_FOCUS_MODE, ANDROID_CAMERA_FOCUS_MODE_AUTO );
	
	if(_cap->isOpened())
		return 1;
	return 0;
  }

/*
 * Class:     com_cvc_tracker_es_CVCTLib
 * Method:    releaseCam
 * Signature: ()V
 */
JNIEXPORT void JNICALL Java_com_cvc_tracker_es_CVCTLib_releaseCam
  (JNIEnv *, jclass)
  {
	LOGI("Java_com_cvc_tracker_es_CVCTLib_releaseCam");
	_cap->release();
	delete _cap;
  }

/*
 * Class:     com_cvc_tracker_es_CVCTLib
 * Method:    grab
 * Signature: ()V
 */
JNIEXPORT void JNICALL Java_com_cvc_tracker_es_CVCTLib_grab
  (JNIEnv *, jclass)
  {
	//LOGI("Java_com_cvc_tracker_es_CVCTLib_grab");
	
    _cap->read(_frameGrabbed); //TODO trocar por grab
    cvtColor(_frameGrabbed, _frameGrabbed, CV_BGR2RGB); // TODO: tentar tirar
  }

/*
 * Class:     com_cvc_tracker_es_CVCTLib
 * Method:    resetTrackingState
 * Signature: ()V
 */
JNIEXPORT void JNICALL Java_com_cvc_tracker_es_CVCTLib_resetTrackingState
  (JNIEnv *, jclass)
  {
	LOGI("Java_com_cvc_tracker_es_CVCTLib_resetTrackingState");
	cvct.restartKLT();
  }

/*
 * Class:     com_cvc_tracker_es_CVCTLib
 * Method:    copyToIMProcessing
 * Signature: ()V
 */
JNIEXPORT void JNICALL Java_com_cvc_tracker_es_CVCTLib_copyToIMProcessing
  (JNIEnv *, jclass)
  {
	LOGI("Java_com_cvc_tracker_es_CVCTLib_copyToIMProcessing");
	if(_frameGrabbed.data != NULL)
		cvtColor(_frameGrabbed, imgPool[currImage], CV_RGB2GRAY);
	
  }
  
  /*
 * Class:     com_cvc_tracker_es_CVCTLib
 * Method:    loadMarker
 * Signature: (J)V
 */
JNIEXPORT void JNICALL Java_com_cvc_tracker_es_CVCTLib_loadMarker
  (JNIEnv *, jclass, jlong addrRgba)
  {
	cv::Mat* pMatRgba=(cv::Mat*)addrRgba;    
    cvct.loadMarkerImage(*pMatRgba);
  }

/*
 * Class:     com_cvc_tracker_es_CVCTLib
 * Method:    process
 * Signature: ()V
 */
JNIEXPORT void JNICALL Java_com_cvc_tracker_es_CVCTLib_process
  (JNIEnv *, jclass, jint isKtl, jint isNatural)
  {
	LOGI("Java_com_cvc_tracker_es_CVCTLib_process");
	if(imgPool[!currImage].data == NULL ||  imgPool[currImage].data == NULL)
	{
		currImage ^= 1;
		return;
	}
	if (isKtl == 0)
		cvct.useKLT(false);
	else
		cvct.useKLT(true);
	
	if (isNatural == 0)
		cvct.useNaturalMarker(false);
	else
		cvct.useNaturalMarker(true);
	LOGI("FAZ TUDO");	
 	cvct.faztudo( imgPool[!currImage], imgPool[currImage] );
    currImage ^= 1; 
  }

/*
 * Class:     com_cvc_tracker_es_CVCTLib
 * Method:    copyMatrixToRender
 * Signature: ()V
 */
JNIEXPORT void JNICALL Java_com_cvc_tracker_es_CVCTLib_copyMatrixToRender
  (JNIEnv *, jclass)
  {
	LOGI("Java_com_cvc_tracker_es_CVCTLib_copyMatrixToRender");
	cvct.getModelViewProjection( mvp );
	
  }
  
  /*
 * Class:     com_cvc_tracker_es_CVCTLib
 * Method:    getMatrixToRender
 * Signature: ()V
 */
JNIEXPORT void JNICALL Java_com_cvc_tracker_es_CVCTLib_getMatrixToRender
  (JNIEnv *, jclass)
  {
	memcpy(mvpRender,mvp,16*sizeof(float));
  }

/*
 * Class:     com_cvc_tracker_es_CVCTLib
 * Method:    init
 * Signature: (II)V
 */
JNIEXPORT void JNICALL Java_com_cvc_tracker_es_CVCTLib_init
  (JNIEnv *, jclass, jint w , jint h )
  {
	//LOGI("Java_com_cvc_tracker_es_CVCTLib_init");
	
	glGenTextures( 1, &_textureGLID );
	glBindTexture( GL_TEXTURE_2D, _textureGLID );
	glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR );
    glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR );    
    glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE );
    glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE );
	int nw = (int)((double)h*4.0/3.0);
	setupGraphics(nw,h);
  }

/*
 * Class:     com_cvc_tracker_es_CVCTLib
 * Method:    copyToRender
 * Signature: ()V
 */
JNIEXPORT void JNICALL Java_com_cvc_tracker_es_CVCTLib_copyToRender
  (JNIEnv *, jclass)
  {
	//LOGI("Java_com_cvc_tracker_es_CVCTLib_copyToRender");	
	if(_frameGrabbed.data != NULL)
	{
	glBindTexture( GL_TEXTURE_2D, _textureGLID );

		glTexImage2D( GL_TEXTURE_2D, 0, GL_RGB, _frameGrabbed.cols, _frameGrabbed.rows, 0, GL_RGB, GL_UNSIGNED_BYTE, (const char*)_frameGrabbed.data );
 	}
  }

/*
 * Class:     com_cvc_tracker_es_CVCTLib
 * Method:    render
 * Signature: ()V
 */
JNIEXPORT void JNICALL Java_com_cvc_tracker_es_CVCTLib_render
  (JNIEnv *, jclass)
  {
	//LOGI("Java_com_cvc_tracker_es_CVCTLib_render");
	static float grey;
    grey += 0.01f;
    if (grey > 1.0f) {
        grey = 0.0f;
    }
    glClearColor(grey, grey, grey, 1.0f);
    checkGlError("glClearColor");
    glClear( GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
    checkGlError("glClear");

    glUseProgram(gProgram);
    checkGlError("glUseProgram");

    glVertexAttribPointer(gvPositionHandle, 2, GL_FLOAT, GL_FALSE, 0, squareVertices);
	glVertexAttribPointer(gvTextureHandle, 2, GL_FLOAT, GL_FALSE, 0, textureVertices);
	glUniform1i(gvSamplerHandle, 0);

	glDisable( GL_DEPTH_TEST );
    glActiveTexture( GL_TEXTURE0 );
    glBindTexture( GL_TEXTURE_2D, _textureGLID );
	
    checkGlError("glVertexAttribPointer");
    glEnableVertexAttribArray(gvPositionHandle);
	glEnableVertexAttribArray(gvTextureHandle);
    checkGlError("glEnableVertexAttribArray");
    glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
    checkGlError("glDrawArrays");
    
    glBindTexture( GL_TEXTURE_2D, 0 );
    glUseProgram( 0 );
    
    glUseProgram(gCubeProgram);
 	glEnableVertexAttribArray(iLocPosition);
    glEnableVertexAttribArray(iLocColour);

    /* Populate attributes for position, colour and texture coordinates etc. */
    glVertexAttribPointer(iLocPosition, 3, GL_FLOAT, GL_FALSE, 0, g_vertex_buffer_data);
    glVertexAttribPointer(iLocColour, 3, GL_FLOAT, GL_FALSE, 0, g_color_buffer_data);

    glEnable(GL_CULL_FACE);
    glEnable(GL_DEPTH_TEST);
    
    float mvph[] = {1.280330, -0.985598, -0.578506, -0.577350, 0.000000, 1.971197, -0.578506, -0.577350, -1.280330, -0.985598, -0.578506, -0.577350, 0.000000, 0.000000, 86.575714, 86.602539};
    glUniformMatrix4fv(iLocMVP, 1, GL_FALSE, mvpRender);

    glDrawArrays(GL_TRIANGLES, 0, 36);
    
    glUseProgram( 0 );
  }

